from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from rclpy import qos
from sls_msgs.msg import UGVsteer,StateUGV
from nav_msgs.msg import Odometry
from sls_msgs.msg import LlaVelocity


# Topic naming convention:
# /ugv/[sensors | actuators]/device_name/[out | in]/msg_name
# This defines the structure for ROS topics used in the UGV interface.
class UGVinterface:
    """Interface for controlling a vehicle."""

    def __init__(self, main_node: Node = None) -> None:
        self.main_node = main_node
        self.id:str = self.main_node.vehicleID
        self.prefix = ''
        self.qos_profile = 1
        if hasattr(self.main_node, 'ugv_simulation_mode'):
            if self.main_node.ugv_simulation_mode:
                self.prefix = '/'+self.id
                self.chassis_pub = main_node.create_publisher(
                    Float64MultiArray,
                    self.prefix + "/actuators/chassis/in/to_can_bus_cmd",
                    qos_profile=self.qos_profile,
                )
                self.ugv_odom_sub = main_node.create_subscription(
                    Odometry,
                    self.prefix +"/out/odom",
                    self.odom_callback,
                    qos_profile=10,
                )
                self.lla_sub = main_node.create_subscription(
                    LlaVelocity,
                    self.prefix +"/sensors/rtk/out/lla",
                    self.lla_msg_callback,
                    qos_profile=qos.qos_profile_sensor_data,
                )
            else:             
                self.chassis_pub = main_node.create_publisher(
                    UGVsteer,
                    "/ugv/cmd/control/in/from_custom",
                    qos_profile=self.qos_profile,
                )
                self.ugv_odom_sub = main_node.create_subscription(
                    Odometry,
                    "/ugv/out/odom",
                    self.odom_callback,
                    qos_profile=10,
                )
                self.lla_sub = main_node.create_subscription(
                    LlaVelocity,
                    "/ugv/sensors/rtk/out/lla",
                    self.lla_msg_callback,
                    qos_profile=qos.qos_profile_sensor_data,
                )
                self.ugv_state_sub = main_node.create_subscription(
                    StateUGV,
                    "/ugv/state/msg/out/ugv_state",
                    self.ugv_state_callback,
                    qos_profile=10,
                )



        self.motor_data = None
        self.steer_data = None
        self.latitude = None
        self.longitude = None
        self.altitude = None
        self.vel = None
        self.yaw = None

        self.vehicle_speed = None
        self.steering_wheel_angle = None
        self.shiftlevel_sts = None
        self.p_sts = None
        self.ignition_sts = None
        self.steering_wheel_direction = None
        self.drive_mode = None

        self.odom_position = None
        self.odom_orientation = None
        self.odom_linear_velocity = None
        self.odom_angular_velocity = None

    def motor_msg_callback(self, msg: Float64MultiArray) -> None:
        """
        Callback function for motor message.
        """
        # print("#DEBUG###")
        self.motor_data = msg.data

    def steer_msg_callback(self, msg: Float64MultiArray) -> None:
        """
        Callback function for steering message.
        """
        self.steer_data = msg.data

    def lla_msg_callback(self, msg: LlaVelocity) -> None:
        """
        Callback function for LLA message.
        """
        self.latitude = msg.latitude
        self.longitude =msg.longitude
        self.altitude = msg.altitude
        self.vel = msg.vel
        self.yaw = msg.yaw

    def ugv_state_callback(self, msg:StateUGV):
        # m/s
        self.vehicle_speed = msg.vehicle_speed
        # [0, 1200]:[0, 20]
        self.steering_wheel_angle = msg.steering_wheel_angle
        # [0:undetected, 1:D, 2:N, 3:R]
        self.shiftlevel_sts = msg.shiftlevel_sts
        # 0:release, 1:apply
        self.p_sts = msg.p_sts
        self.ignition_sts = msg.ignition_sts
        # 0:left, 1:right
        self.steering_wheel_direction = msg.steering_wheel_direction
        # 0:?, 1:auto, 2:PAD, 3:steer-wheel
        self.drive_mode = msg.drive_mode

    def odom_callback(self, msg:Odometry):
        # x,y,z
        self.odom_position = msg.pose.pose.position
        # q_x,q_y,q_z,q_w
        self.odom_orientation = msg.pose.pose.orientation
        self.odom_linear_velocity = msg.twist.twist.linear
        self.odom_angular_velocity = msg.twist.twist.angular

    def publish_chassis(self, shift_level_reg:int=127,target_speed:float=127.0,steering_wheel_angle:float=127.0,
                     drive_mode_reg:int=127,brk_en:int=127,brake_mode:int=127,acc_mode:int=127) -> None:
        """
        Publish chassis data
        """
        msg = UGVsteer()
        msg.shift_level_reg = shift_level_reg
        msg.target_speed = float(target_speed)
        msg.steering_wheel_angle = float(steering_wheel_angle)
        msg.drive_mode_reg = drive_mode_reg
        msg.brk_en = brk_en
        msg.brake_mode = brake_mode
        msg.acc_mode = acc_mode
        # Publish the message
        self.chassis_pub.publish(msg)
        # self.main_node.get_logger().debug(f"publish chassis data: {msg.data}")
    
    # [mode, shift, steer(rad/s), speed(m/s), brake]
    def simu_public_classis(self, shift_level_reg:int=127,target_speed:float=127.0,steering_wheel_angle:float=127.0,
                     drive_mode_reg:int=127,brk_en:int=127,brake_mode:int=127,acc_mode:int=127) -> None:
        cmd_msg = Float64MultiArray()
        cmd_msg.data = [drive_mode_reg, shift_level_reg, steering_wheel_angle, target_speed, brk_en]
        self.chassis_pub.publish(cmd_msg)
